/*
 * Test of one motor: gradually increase the speed of the motor 0 (CON11) and
 * display the ticks from the encoder
 */

# include "init_systems.h"
# include "log.h"
# include <rrcon.h>
# include <time_tools.h>

# define MAXSPEED 1023 // Maximum speed of the motors
# define NMOTOR 3      // Motor slot to test 0->CON11 1->CON13 2->CON12 3->CON14

using namespace kthrobot;
using namespace std;

int main (int argc, char **argv) {
    // Init
    Init::RegisterCommon();
    RRCon::Register();
    if (!Init::Systems(argc, argv)) return 1;
    srand(time(NULL));

    // Initializing the motor
    Motor& motor = RRCon::motor(NMOTOR);
    motor.SetEncToZero();
    // Firing up the motor
    EncoderData ticks;
    int mes_speed = 0;
    for(int speed=0; speed<MAXSPEED; speed=speed+100) {
        Log::Write("Setting speed to: %i",speed);
        motor.SetSpeed(speed);
        // Logging the measured ticks
        for(int i=0; i<=10; i++) {
            TimeTools::WaitMS(50);
            ticks = motor.ReadEnc();
            mes_speed = ticks.cur - ticks.prev;
            Log::Write("Number of tics: %i, Speed: %i tics/s",ticks.cur,speed);
        }
    }
    motor.SetSpeed(0);

    // Shutdown
    Init::Shutdown();
    return 0;
}
